﻿using System;
// ReSharper disable All

namespace ModbusSlave
{
    #region class ModbusProtocol
    /// <summary>
    /// Modbus Protocol informations.
    /// </summary>
    public class ModbusProtocol
    {
        public DateTime timeStamp;
        public bool request;
        public bool response;
        public UInt16 transactionIdentifier;
        public UInt16 protocolIdentifier;
        public UInt16 length;
        public byte unitIdentifier; // 设备地址
        public byte functionCode;
        public UInt16 startingAdress;
        public UInt16 startingAddressRead;
        public UInt16 startingAddressWrite;
        public UInt16 quantity;
        public UInt16 quantityRead;
        public UInt16 quantityWrite;
        public byte byteCount;
        public byte exceptionCode;
        public byte errorCode;
        public UInt16[] receiveCoilValues;
        public UInt16[] receiveRegisterValues;
        public Int16[] sendRegisterValues;
        public bool[] sendCoilValues;
        public UInt16 crc;
    }
    #endregion


    /// <summary>
    /// Modbus TCP Server.
    /// </summary>
    public class ModbusServer
    {
        public HoldingRegisters holdingRegisters;
        public InputRegisters inputRegisters;
        public Coils coils;
        public DiscreteInputs discreteInputs;
        private int numberOfConnections = 0;
        private bool udpFlag;
        private bool serialFlag;
        private byte unitIdentifier = 1;


        private ModbusProtocol[] modbusLogData = new ModbusProtocol[100];
        public bool FunctionCode1Disabled { get; set; }
        public bool FunctionCode2Disabled { get; set; }
        public bool FunctionCode3Disabled { get; set; }
        public bool FunctionCode4Disabled { get; set; }
        public bool FunctionCode5Disabled { get; set; }
        public bool FunctionCode6Disabled { get; set; }
        public bool FunctionCode15Disabled { get; set; }
        public bool FunctionCode16Disabled { get; set; }
        public bool FunctionCode23Disabled { get; set; }
        public bool PortChanged { get; set; }
        private readonly object lockCoils = new object();
        private readonly object lockHoldingRegisters = new object();


        public ModbusServer()
        {
            holdingRegisters = new HoldingRegisters(this);
            inputRegisters = new InputRegisters(this);
            coils = new Coils(this);
            discreteInputs = new DiscreteInputs(this);

        }

        #region events
        public delegate bool CoilsChangedHandler(int coil, int numberOfCoils, byte function);
        public event CoilsChangedHandler CoilsChanged;

        public delegate bool HoldingRegistersChangedHandler(int register, int numberOfRegisters);
        public event HoldingRegistersChangedHandler HoldingRegistersChanged;
        #endregion


        private readonly object _synObj = new object();
        #region Method ProcessReceivedData
        public byte[] ProcessReceivedData(byte[] recBytes)
        {
            if (recBytes == null || recBytes.Length < 5) return null;
            lock (_synObj)
            {
                ModbusProtocol receiveDataThread = new ModbusProtocol();
                ModbusProtocol sendDataThread = new ModbusProtocol();

                try
                {
                    UInt16[] wordData = new UInt16[1];
                    byte[] byteData = new byte[2];
                    receiveDataThread.timeStamp = DateTime.Now;
                    receiveDataThread.request = true;
                    if (!serialFlag)
                    {
                        //Lese Transaction identifier
                        byteData[1] = recBytes[0];
                        byteData[0] = recBytes[1];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.transactionIdentifier = wordData[0];

                        //Lese Protocol identifier
                        byteData[1] = recBytes[2];
                        byteData[0] = recBytes[3];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.protocolIdentifier = wordData[0];

                        //Lese length
                        byteData[1] = recBytes[4];
                        byteData[0] = recBytes[5];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.length = wordData[0];
                    }

                    //Lese unit identifier
                    receiveDataThread.unitIdentifier = recBytes[6 - 6 * Convert.ToInt32(serialFlag)];
                    //Check UnitIdentifier
                    if ((receiveDataThread.unitIdentifier != this.unitIdentifier) &
                        (receiveDataThread.unitIdentifier != 0)) // 地址检查
                        return null;

                    // Lese function code
                    receiveDataThread.functionCode = recBytes[7 - 6 * Convert.ToInt32(serialFlag)]; // 功能码

                    // Lese starting address 
                    byteData[1] = recBytes[8 - 6 * Convert.ToInt32(serialFlag)];
                    byteData[0] = recBytes[9 - 6 * Convert.ToInt32(serialFlag)];
                    Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                    receiveDataThread.startingAdress = wordData[0];

                    if (receiveDataThread.functionCode <= 4)
                    {
                        // Lese quantity
                        byteData[1] = recBytes[10 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[11 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.quantity = wordData[0];
                    }
                    else if (receiveDataThread.functionCode == 5)
                    {
                        receiveDataThread.receiveCoilValues = new ushort[1];
                        // Lese Value
                        byteData[1] = recBytes[10 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[11 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, receiveDataThread.receiveCoilValues, 0, 2);
                    }
                    else if (receiveDataThread.functionCode == 6)
                    {
                        receiveDataThread.receiveRegisterValues = new ushort[1];
                        // Lese Value
                        byteData[1] = recBytes[10 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[11 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, receiveDataThread.receiveRegisterValues, 0, 2);
                    }
                    else if (receiveDataThread.functionCode == 15)
                    {
                        // Lese quantity
                        byteData[1] = recBytes[10 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[11 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.quantity = wordData[0];

                        receiveDataThread.byteCount = recBytes[12 - 6 * Convert.ToInt32(serialFlag)];

                        if ((receiveDataThread.byteCount % 2) != 0)
                            receiveDataThread.receiveCoilValues = new ushort[receiveDataThread.byteCount / 2 + 1];
                        else
                            receiveDataThread.receiveCoilValues = new ushort[receiveDataThread.byteCount / 2];
                        // Lese Value
                        Buffer.BlockCopy(recBytes, 13 - 6 * Convert.ToInt32(serialFlag),
                            receiveDataThread.receiveCoilValues, 0, receiveDataThread.byteCount);
                    }
                    else if (receiveDataThread.functionCode == 16)
                    {
                        // Lese quantity
                        byteData[1] = recBytes[10 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[11 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.quantity = wordData[0];

                        receiveDataThread.byteCount = recBytes[12 - 6 * Convert.ToInt32(serialFlag)];
                        receiveDataThread.receiveRegisterValues = new ushort[receiveDataThread.quantity];
                        for (int i = 0; i < receiveDataThread.quantity; i++)
                        {
                            // Lese Value
                            byteData[1] = recBytes[13 + i * 2 - 6 * Convert.ToInt32(serialFlag)];
                            byteData[0] = recBytes[14 + i * 2 - 6 * Convert.ToInt32(serialFlag)];
                            Buffer.BlockCopy(byteData, 0, receiveDataThread.receiveRegisterValues, i * 2, 2);
                        }

                    }
                    else if (receiveDataThread.functionCode == 23)
                    {
                        // Lese starting Address Read
                        byteData[1] = recBytes[8 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[9 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.startingAddressRead = wordData[0];
                        // Lese quantity Read
                        byteData[1] = recBytes[10 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[11 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.quantityRead = wordData[0];
                        // Lese starting Address Write
                        byteData[1] = recBytes[12 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[13 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.startingAddressWrite = wordData[0];
                        // Lese quantity Write
                        byteData[1] = recBytes[14 - 6 * Convert.ToInt32(serialFlag)];
                        byteData[0] = recBytes[15 - 6 * Convert.ToInt32(serialFlag)];
                        Buffer.BlockCopy(byteData, 0, wordData, 0, 2);
                        receiveDataThread.quantityWrite = wordData[0];

                        receiveDataThread.byteCount = recBytes[16 - 6 * Convert.ToInt32(serialFlag)];
                        receiveDataThread.receiveRegisterValues = new ushort[receiveDataThread.quantityWrite];
                        for (int i = 0; i < receiveDataThread.quantityWrite; i++)
                        {
                            // Lese Value
                            byteData[1] = recBytes[17 + i * 2 - 6 * Convert.ToInt32(serialFlag)];
                            byteData[0] = recBytes[18 + i * 2 - 6 * Convert.ToInt32(serialFlag)];
                            Buffer.BlockCopy(byteData, 0, receiveDataThread.receiveRegisterValues, i * 2, 2);
                        }
                    }
                    else
                    {
                        return null;
                    }
                }
                catch
                {
                    return null;
                }
                return this.CreateAnswer(receiveDataThread, sendDataThread);
                //this.sendAnswer();
                // this.CreateLogData(receiveDataThread, sendDataThread);
            }
        }
        #endregion

        #region Method CreateAnswer
        private byte[] CreateAnswer(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.timeStamp = DateTime.Now;
            switch (recData.functionCode)
            {
                // Read Coils
                case 1:
                    if (!FunctionCode1Disabled)
                        return this.ReadCoils(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Read Input Registers
                case 2:
                    if (!FunctionCode2Disabled)
                        return this.ReadDiscreteInputs(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Read Holding Registers
                case 3:
                    if (!FunctionCode3Disabled)
                        return this.ReadHoldingRegisters(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Read Input Registers
                case 4:
                    if (!FunctionCode4Disabled)
                        return this.ReadInputRegisters(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Write single coil
                case 5:
                    if (!FunctionCode5Disabled)
                        return this.WriteSingleCoil(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Write single register
                case 6:
                    if (!FunctionCode6Disabled)
                        return this.WriteSingleRegister(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Write Multiple coils
                case 15:
                    if (!FunctionCode15Disabled)
                        return this.WriteMultipleCoils(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Write Multiple registers
                case 16:
                    if (!FunctionCode16Disabled)
                        return this.WriteMultipleRegisters(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Error: Function Code not supported
                case 23:
                    if (!FunctionCode23Disabled)
                        return this.ReadWriteMultipleRegisters(recData, sndData);
                    else
                    {
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 1;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                // Error: Function Code not supported
                default:
                    sndData.errorCode = (byte)(recData.functionCode + 0x80);
                    sndData.exceptionCode = 1;
                    return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
            }

        }
        #endregion

        private byte[] ReadCoils(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            if ((recData.quantity < 1) | (recData.quantity > 0x07D0))  //Invalid quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if (((recData.startingAdress + 1 + recData.quantity) > 65535) | (recData.startingAdress < 0))     //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                if ((recData.quantity % 8) == 0)
                    sndData.byteCount = (byte)(recData.quantity / 8);
                else
                    sndData.byteCount = (byte)(recData.quantity / 8 + 1);

                sndData.sendCoilValues = new bool[recData.quantity];
                lock (lockCoils)
                    Array.Copy(coils.localArray, recData.startingAdress + 1, sndData.sendCoilValues, 0, recData.quantity);
            }

            Byte[] data;

            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[9 + sndData.byteCount + 2 * Convert.ToInt32(serialFlag)];

            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            Byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];
            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;

            //ByteCount
            data[8] = sndData.byteCount;

            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendCoilValues = null;
            }

            if (sndData.sendCoilValues != null)
                for (int i = 0; i < (sndData.byteCount); i++)
                {
                    byteData = new byte[2];
                    for (int j = 0; j < 8; j++)
                    {

                        byte boolValue;
                        if (sndData.sendCoilValues[i * 8 + j] == true)
                            boolValue = 1;
                        else
                            boolValue = 0;
                        byteData[1] = (byte)((byteData[1]) | (boolValue << j));
                        if ((i * 8 + j + 1) >= sndData.sendCoilValues.Length)
                            break;
                    }
                    data[9 + i] = byteData[1];
                }
            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }

            return null;
        }

        private byte[] ReadDiscreteInputs(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            if ((recData.quantity < 1) | (recData.quantity > 0x07D0))  //Invalid quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if (((recData.startingAdress + 1 + recData.quantity) > 65535) | (recData.startingAdress < 0))   //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                if ((recData.quantity % 8) == 0)
                    sndData.byteCount = (byte)(recData.quantity / 8);
                else
                    sndData.byteCount = (byte)(recData.quantity / 8 + 1);

                sndData.sendCoilValues = new bool[recData.quantity];
                Array.Copy(discreteInputs.localArray, recData.startingAdress + 1, sndData.sendCoilValues, 0, recData.quantity);
            }

            Byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[9 + sndData.byteCount + 2 * Convert.ToInt32(serialFlag)];
            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;

            //ByteCount
            data[8] = sndData.byteCount;


            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendCoilValues = null;
            }

            if (sndData.sendCoilValues != null)
                for (int i = 0; i < (sndData.byteCount); i++)
                {
                    byteData = new byte[2];
                    for (int j = 0; j < 8; j++)
                    {

                        byte boolValue;
                        if (sndData.sendCoilValues[i * 8 + j] == true)
                            boolValue = 1;
                        else
                            boolValue = 0;
                        byteData[1] = (byte)((byteData[1]) | (boolValue << j));
                        if ((i * 8 + j + 1) >= sndData.sendCoilValues.Length)
                            break;
                    }
                    data[9 + i] = byteData[1];
                }

            try
            {
                if (serialFlag)
                {

                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }
            return null;
        }

        private byte[] ReadHoldingRegisters(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            if ((recData.quantity < 1) | (recData.quantity > 0x007D))  //Invalid quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if (((recData.startingAdress + 1 + recData.quantity) > 65535) | (recData.startingAdress < 0))   //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                sndData.byteCount = (byte)(2 * recData.quantity);
                sndData.sendRegisterValues = new Int16[recData.quantity];
                lock (lockHoldingRegisters)
                    Buffer.BlockCopy(holdingRegisters.localArray, recData.startingAdress * 2 + 2, sndData.sendRegisterValues, 0, recData.quantity * 2);
            }
            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = (ushort)(0x03 + sndData.byteCount);


            Byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[9 + sndData.byteCount + 2 * Convert.ToInt32(serialFlag)];
            Byte[] byteData = new byte[2];
            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;

            //ByteCount
            data[8] = sndData.byteCount;

            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendRegisterValues = null;
            }


            //if (sndData.sendRegisterValues != null)
            //    for (int i = 0; i < (sndData.byteCount / 2); i++)
            //    {
            //        byteData = BitConverter.GetBytes((Int16)sndData.sendRegisterValues[i]);
            //        data[9 + i * 2] = byteData[1];
            //        data[10 + i * 2] = byteData[0];
            //    }

            if (sndData.sendRegisterValues != null)
                for (int i = 0; i < (sndData.byteCount / 4); i++)
                {
                    byteData = BitConverter.GetBytes((Int16)sndData.sendRegisterValues[i * 2 + 1]);
                    data[9 + i * 4] = byteData[1];
                    data[10 + i * 4] = byteData[0];
                    byteData = BitConverter.GetBytes((Int16)sndData.sendRegisterValues[i * 2]);
                    data[9 + i * 4 + 2] = byteData[1];
                    data[10 + i * 4 + 2] = byteData[0];
                }
            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }

            return null;
        }

        private byte[] ReadInputRegisters(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            if ((recData.quantity < 1) | (recData.quantity > 0x007D))  //Invalid quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if (((recData.startingAdress + 1 + recData.quantity) > 65535) | (recData.startingAdress < 0))   //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                sndData.byteCount = (byte)(2 * recData.quantity);
                sndData.sendRegisterValues = new Int16[recData.quantity];
                Buffer.BlockCopy(inputRegisters.localArray, recData.startingAdress * 2 + 2, sndData.sendRegisterValues, 0, recData.quantity * 2);
            }
            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = (ushort)(0x03 + sndData.byteCount);


            Byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[9 + sndData.byteCount + 2 * Convert.ToInt32(serialFlag)];
            Byte[] byteData = new byte[2];
            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;

            //ByteCount
            data[8] = sndData.byteCount;


            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendRegisterValues = null;
            }


            //if (sndData.sendRegisterValues != null)
            //    for (int i = 0; i < (sndData.byteCount / 2); i++)
            //    {
            //        byteData = BitConverter.GetBytes((Int16)sndData.sendRegisterValues[i]);
            //        data[9 + i * 2] = byteData[1];
            //        data[10 + i * 2] = byteData[0];
            //    }
            if (sndData.sendRegisterValues != null)
                for (int i = 0; i < (sndData.byteCount / 4); i++)
                {
                    byteData = BitConverter.GetBytes((Int16)sndData.sendRegisterValues[i * 2 + 1]);
                    data[9 + i * 4] = byteData[1];
                    data[10 + i * 4] = byteData[0];
                    byteData = BitConverter.GetBytes((Int16)sndData.sendRegisterValues[i * 2]);
                    data[9 + i * 4 + 2] = byteData[1];
                    data[10 + i * 4 + 2] = byteData[0];
                }
            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);

                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }

            return null;
        }

        private byte[] WriteSingleCoil(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            sndData.startingAdress = recData.startingAdress;
            sndData.receiveCoilValues = recData.receiveCoilValues;
            if ((recData.receiveCoilValues[0] != 0x0000) & (recData.receiveCoilValues[0] != 0xFF00))  //Invalid Value
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if (((recData.startingAdress + 1) > 65535) | (recData.startingAdress < 0))    //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                if (recData.receiveCoilValues[0] == 0xFF00)
                {
                    lock (lockCoils)
                    {
                        bool swapVal = coils[recData.startingAdress + 1];
                        coils[recData.startingAdress + 1] = true;
                        if (CoilsChanged?.Invoke(recData.startingAdress + 1, 1, 0x05) == false) // 修改失败
                        {
                            coils[recData.startingAdress + 1] = swapVal;
                            sndData.errorCode = (byte)(recData.functionCode + 0x80);
                            sndData.exceptionCode = 3;
                            return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                        }
                    }
                }
                if (recData.receiveCoilValues[0] == 0x0000)
                {
                    lock (lockCoils)
                    {
                        bool swapVal = coils[recData.startingAdress + 1];
                        coils[recData.startingAdress + 1] = false;
                        if (CoilsChanged?.Invoke(recData.startingAdress + 1, 1, 0x05) == false) // 修改失败
                        {
                            coils[recData.startingAdress + 1] = swapVal;
                            sndData.errorCode = (byte)(recData.functionCode + 0x80);
                            sndData.exceptionCode = 3;
                            return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                        }
                    }
                }

            }
            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = 0x06;


            byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[12 + 2 * Convert.ToInt32(serialFlag)];

            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;



            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendRegisterValues = null;
            }
            else
            {
                byteData = BitConverter.GetBytes((int)recData.startingAdress);
                data[8] = byteData[1];
                data[9] = byteData[0];
                byteData = BitConverter.GetBytes((int)recData.receiveCoilValues[0]);
                data[10] = byteData[1];
                data[11] = byteData[0];
            }


            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;

                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }

            return null;

        }

        private byte[] WriteSingleRegister(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            sndData.startingAdress = recData.startingAdress;
            sndData.receiveRegisterValues = recData.receiveRegisterValues;

            if ((recData.receiveRegisterValues[0] < 0x0000) | (recData.receiveRegisterValues[0] > 0xFFFF))  //Invalid Value
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if (((recData.startingAdress + 1) > 65535) | (recData.startingAdress < 0))    //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                lock (lockHoldingRegisters)
                {
                    Int16 swapVal = holdingRegisters[recData.startingAdress + 1];
                    holdingRegisters[recData.startingAdress + 1] = unchecked((short)recData.receiveRegisterValues[0]);
                    if (HoldingRegistersChanged?.Invoke(recData.startingAdress + 1, 1) == false) // 修改失败
                    {
                        holdingRegisters[recData.startingAdress + 1] = swapVal;
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 3;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                }
            }
            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = 0x06;


            byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[12 + 2 * Convert.ToInt32(serialFlag)];

            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;


            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendRegisterValues = null;
            }
            else
            {
                byteData = BitConverter.GetBytes((int)recData.startingAdress);
                data[8] = byteData[1];
                data[9] = byteData[0];
                byteData = BitConverter.GetBytes((int)recData.receiveRegisterValues[0]);
                data[10] = byteData[1];
                data[11] = byteData[0];
            }


            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }


            return null;
        }

        private byte[] WriteMultipleCoils(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            sndData.startingAdress = recData.startingAdress;
            sndData.quantity = recData.quantity;

            if ((recData.quantity == 0x0000) | (recData.quantity > 0x07B0))  //Invalid Quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if ((((int)recData.startingAdress + 1 + (int)recData.quantity) > 65535) | (recData.startingAdress < 0))    //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                lock (lockCoils)
                {
                    bool[] swapVals = new bool[recData.quantity];
                    for (int i = 0; i < recData.quantity; i++)
                    {
                        swapVals[i] = coils[recData.startingAdress + i + 1];
                    }
                    for (int i = 0; i < recData.quantity; i++)
                    {
                        int shift = i % 16;
                        /*                if ((i == receiveData.quantity - 1) & (receiveData.quantity % 2 != 0))
                                        {
                                            if (shift < 8)
                                                shift = shift + 8;
                                            else
                                                shift = shift - 8;
                                        }*/
                        int mask = 0x1;
                        mask = mask << (shift);
                        if ((recData.receiveCoilValues[i / 16] & (ushort)mask) == 0)
                            coils[recData.startingAdress + i + 1] = false;
                        else
                            coils[recData.startingAdress + i + 1] = true;
                    }

                    if (CoilsChanged?.Invoke(recData.startingAdress + 1, recData.quantity, 0x0F) == false) // 修改失败
                    {
                        for (int i = 0; i < recData.quantity; i++)
                        {
                            coils[recData.startingAdress + i + 1] = swapVals[i];
                        }
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 3;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                }
            }
            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = 0x06;

            byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[12 + 2 * Convert.ToInt32(serialFlag)];


            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;



            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendRegisterValues = null;
            }
            else
            {
                byteData = BitConverter.GetBytes((int)recData.startingAdress);
                data[8] = byteData[1];
                data[9] = byteData[0];
                byteData = BitConverter.GetBytes((int)recData.quantity);
                data[10] = byteData[1];
                data[11] = byteData[0];
            }


            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;

                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }
            return null;
        }

        private byte[] WriteMultipleRegisters(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;
            sndData.startingAdress = recData.startingAdress;
            sndData.quantity = recData.quantity;

            if ((recData.quantity == 0x0000) | (recData.quantity > 0x07B0))  //Invalid Quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if ((((int)recData.startingAdress + 1 + (int)recData.quantity) > 65535) | (recData.startingAdress < 0))   //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                lock (lockHoldingRegisters)
                {
                    Int16[] swapVals = new Int16[recData.quantity];
                    for (int i = 0; i < recData.quantity; i++)
                    {
                        swapVals[i] = holdingRegisters[recData.startingAdress + i + 1];
                    }
                    for (int i = 0; i < recData.quantity; i++)
                    {
                        holdingRegisters[recData.startingAdress + i + 1] = unchecked((short)recData.receiveRegisterValues[i]);
                    }

                    if (HoldingRegistersChanged?.Invoke(recData.startingAdress + 1, recData.quantity) == false) // 修改失败
                    {
                        for (int i = 0; i < recData.quantity; i++)
                        {
                            holdingRegisters[recData.startingAdress + i + 1] = swapVals[i];
                        }
                        sndData.errorCode = (byte)(recData.functionCode + 0x80);
                        sndData.exceptionCode = 3;
                        return sendException(sndData.errorCode, sndData.exceptionCode, recData, sndData);
                    }
                }
            }
            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = 0x06;

            byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[12 + 2 * Convert.ToInt32(serialFlag)];

            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;



            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendRegisterValues = null;
            }
            else
            {
                byteData = BitConverter.GetBytes((int)recData.startingAdress);
                data[8] = byteData[1];
                data[9] = byteData[0];
                byteData = BitConverter.GetBytes((int)recData.quantity);
                data[10] = byteData[1];
                data[11] = byteData[0];
            }


            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }

            return null;
        }

        private byte[] ReadWriteMultipleRegisters(ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = this.unitIdentifier;
            sndData.functionCode = recData.functionCode;


            if ((recData.quantityRead < 0x0001) | (recData.quantityRead > 0x007D) | (recData.quantityWrite < 0x0001) | (recData.quantityWrite > 0x0079) | (recData.byteCount != (recData.quantityWrite * 2)))  //Invalid Quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 3;
            }
            if ((((int)recData.startingAddressRead + 1 + (int)recData.quantityRead) > 65535) | (((int)recData.startingAddressWrite + 1 + (int)recData.quantityWrite) > 65535) | (recData.quantityWrite < 0) | (recData.quantityRead < 0))    //Invalid Starting adress or Starting address + quantity
            {
                sndData.errorCode = (byte)(recData.functionCode + 0x80);
                sndData.exceptionCode = 2;
            }
            if (sndData.exceptionCode == 0)
            {
                sndData.sendRegisterValues = new Int16[recData.quantityRead];
                lock (lockHoldingRegisters)
                    Buffer.BlockCopy(holdingRegisters.localArray, recData.startingAddressRead * 2 + 2, sndData.sendRegisterValues, 0, recData.quantityRead * 2);

                lock (holdingRegisters)
                    for (int i = 0; i < recData.quantityWrite; i++)
                    {
                        holdingRegisters[recData.startingAddressWrite + i + 1] = unchecked((short)recData.receiveRegisterValues[i]);
                    }
                sndData.byteCount = (byte)(2 * recData.quantityRead);
            }
            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = Convert.ToUInt16(3 + 2 * recData.quantityRead);

            byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[9 + sndData.byteCount + 2 * Convert.ToInt32(serialFlag)];

            //Send Transaction identifier
            byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;

            //Function Code
            data[7] = sndData.functionCode;

            //ByteCount
            data[8] = sndData.byteCount;


            if (sndData.exceptionCode > 0)
            {
                data[7] = sndData.errorCode;
                data[8] = sndData.exceptionCode;
                sndData.sendRegisterValues = null;
            }
            else
            {
                if (sndData.sendRegisterValues != null)
                    for (int i = 0; i < (sndData.byteCount / 2); i++)
                    {
                        byteData = BitConverter.GetBytes((Int16)sndData.sendRegisterValues[i]);
                        data[9 + i * 2] = byteData[1];
                        data[10 + i * 2] = byteData[0];
                    }

            }


            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }

            HoldingRegistersChanged?.Invoke(recData.startingAddressWrite + 1, recData.quantityWrite);
            return null;
        }

        private byte[] sendException(int errorCode, int exceptionCode, ModbusProtocol recData, ModbusProtocol sndData)
        {
            sndData.response = true;

            sndData.transactionIdentifier = recData.transactionIdentifier;
            sndData.protocolIdentifier = recData.protocolIdentifier;

            sndData.unitIdentifier = recData.unitIdentifier;
            sndData.errorCode = (byte)errorCode;
            sndData.exceptionCode = (byte)exceptionCode;

            if (sndData.exceptionCode > 0)
                sndData.length = 0x03;
            else
                sndData.length = (ushort)(0x03 + sndData.byteCount);


            byte[] data;
            if (sndData.exceptionCode > 0)
                data = new byte[9 + 2 * Convert.ToInt32(serialFlag)];
            else
                data = new byte[9 + sndData.byteCount + 2 * Convert.ToInt32(serialFlag)];
            sndData.length = (byte)(data.Length - 6);

            //Send Transaction identifier
            byte[] byteData = BitConverter.GetBytes((int)sndData.transactionIdentifier);
            data[0] = byteData[1];
            data[1] = byteData[0];

            //Send Protocol identifier
            byteData = BitConverter.GetBytes((int)sndData.protocolIdentifier);
            data[2] = byteData[1];
            data[3] = byteData[0];

            //Send length
            byteData = BitConverter.GetBytes((int)sndData.length);
            data[4] = byteData[1];
            data[5] = byteData[0];

            //Unit Identifier
            data[6] = sndData.unitIdentifier;


            data[7] = sndData.errorCode;
            data[8] = sndData.exceptionCode;


            try
            {
                if (serialFlag)
                {
                    //Create CRC
                    sndData.crc = ModbusHelper.calculateCRC(data, Convert.ToUInt16(data.Length - 8), 6);
                    byteData = BitConverter.GetBytes((int)sndData.crc);
                    data[data.Length - 2] = byteData[0];
                    data[data.Length - 1] = byteData[1];

                    byte[] debugData = new byte[data.Length - 6];
                    Array.Copy(data, 6, debugData, 0, data.Length - 6);
                    return debugData;
                }
                else
                {
                    return data;
                }
            }
            catch (Exception) { }

            return null;
        }


        public int NumberOfConnections => numberOfConnections;

        public ModbusProtocol[] ModbusLogData => modbusLogData;

        public bool UDPFlag
        {
            get => udpFlag;
            set => udpFlag = value;
        }

        public bool SerialFlag
        {
            get => serialFlag;
            set => serialFlag = value;
        }

        public byte UnitIdentifier
        {
            get => unitIdentifier;
            set => unitIdentifier = value;
        }


        public class HoldingRegisters
        {
            public Int16[] localArray = new Int16[65535];
            private ModbusServer modbusServer;

            public HoldingRegisters(ModbusServer modbusServer)
            {
                this.modbusServer = modbusServer;
            }

            public Int16 this[int x]
            {
                get => this.localArray[x];
                set => this.localArray[x] = value;
            }

            public Int32 this[int x, int count = 2]
            {
                get
                {
                    Int32 data = this.localArray[x + 2];
                    return data << 16 + this.localArray[x + 1];
                }
                set
                {
                    Int32 val = value;
                    this.localArray[x + 1] = (Int16)(val & 0x0000FFFF);
                    this.localArray[x + 2] = (Int16)(val >> 16);
                }
            }
        }

        public class InputRegisters
        {
            public Int16[] localArray = new Int16[65535];
            private ModbusServer modbusServer;

            public InputRegisters(ModbusServer modbusServer)
            {
                this.modbusServer = modbusServer;
            }

            public Int16 this[int x]
            {
                get => this.localArray[x];
                set => this.localArray[x] = value;
            }

            public Int32 this[int x, int count = 2]
            {
                get
                {
                    Int32 data = this.localArray[x + 2];
                    return data << 16 + this.localArray[x + 1];
                }
                set
                {
                    Int32 val = value;
                    this.localArray[x + 1] = (Int16)(val & 0x0000FFFF);
                    this.localArray[x + 2] = (Int16)(val >> 16);
                }
            }
        }

        public class Coils
        {
            public bool[] localArray = new bool[65535];
            private ModbusServer modbusServer;

            public Coils(ModbusServer modbusServer)
            {
                this.modbusServer = modbusServer;
            }

            public bool this[int x]
            {
                get => this.localArray[x];
                set => this.localArray[x] = value;
            }
        }

        public class DiscreteInputs
        {
            public bool[] localArray = new bool[65535];
            private ModbusServer modbusServer;

            public DiscreteInputs(ModbusServer modbusServer)
            {
                this.modbusServer = modbusServer;
            }

            public bool this[int x]
            {
                get => this.localArray[x];
                set => this.localArray[x] = value;
            }


        }
    }
}
